// (C) Szymon Piechowicz, February 2012

#include <avr/interrupt.h>
#include <util/delay.h>
#include "adc_manager.h"
#include "common.h"
#include "battery.h"
#include "hall_sensor.h"
#include "lights.h"
#include "motors.h"
#include "obstacle_detector.h"
#include "rc.h"


void brightness_callback(bool is_bright);
void obstacle_callback(bool is_obstacle_detected);


volatile bool is_facing_obstacle = false;


int main(void)
{
  // All ports are pulled-up inputs by default.
  DDRA = DDRB = DDRC = DDRD = 0;
  PORTA = PORTB = PORTC = PORTD = 0xFF;
  
  adc_manager_init();
  battery_init();
  hall_sensor_init();
  lights_init();
  motors_init();
  obstacle_detector_init();
  rc_init();
  
  lights_start_measuring_brightness(brightness_callback);
  battery_start_measuring_charge(NULL);
  obstacle_detector_start(obstacle_callback);
  
  _delay_ms(1000); // let obstacle detector stabilize
  sei();
  _delay_ms(5000); // wait
  motors_both_set_speed(70);
  motors_both_forward();
  while (1)
  {
    if (is_facing_obstacle)
    {
      lights_directionals_l_blinking_on();
      motors_both_stop();
      _delay_ms(200);
      motors_l_set_speed(25);
      motors_r_set_speed(100);
      motors_both_backward();
      while (is_facing_obstacle);
      _delay_ms(500);
      motors_both_stop();
      _delay_ms(200);
      lights_directionals_l_blinking_off();
      motors_both_set_speed(70);
      motors_both_forward();
    }
  }
}

void brightness_callback(bool is_bright)
{
  if (is_bright)
    lights_off();
  else
    lights_on();
}

void obstacle_callback(bool is_obstacle_detected)
{
  is_facing_obstacle = is_obstacle_detected;
}